#include <opencv/cv.h>
#include <opencv/cvwimage.h>
#include <opencv/cvaux.h>
#include <opencv/cxcore.h>
#include <opencv/cxmisc.h>
#include <opencv/highgui.h>
#include <opencv/ml.h>
#include <stdio.h>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <opencv2/opencv.hpp>
#include "detecter.h"

void getError (int ret);

using namespace cv;

void ItemDetecter::reframeBoundaries(Mat imgIn,Rect& roi) const
{
  // check for lower bounds
  if (roi.x<0)
    roi.x=0;
  if (roi.y<0)
    roi.y=0;
  // check for upper bounds
  if (roi.x+roi.width>imgIn.cols)
    roi.width=imgIn.cols-roi.x;
  if (roi.y+roi.height>imgIn.rows)
    roi.height=imgIn.rows-roi.y;
}

Rect ItemDetecter::growROI (int factor) const
{
  Rect grownROI=ROI;
  Point factorDecay(factor/2,factor/2);
  Size factorGrow(factor,factor);
  grownROI=grownROI-factorDecay;
  grownROI=grownROI+factorGrow;
  return grownROI;
}

void ItemDetecter::drawROI(Mat imgIn) const
{
  rectangle(imgIn,ROI,Scalar(0,255,255),-1,8);
}

int ItemDetecter::detectAndReframe( Mat imgIn)
{
  //****************************************************
  //****************************************************
  // TODO: this function should reframe well and set size to a constant
  //****************************************************
  //****************************************************

  bool inputImageAltered=false;
  Mat relevantImgIn=imgIn;

  if (isTracking)
    {
      // printf("We are tracking\n");
      inputImageAltered=true;
      // if we are tracking we only consider a grown ROI of the image in input
      Rect grownROI=growROI(500);
      reframeBoundaries(imgIn,grownROI);
      // Rect grownROI=ROI;
      relevantImgIn=imgIn(grownROI);
    }

  imwrite("test.jpg",relevantImgIn);
  std::vector<Rect> roisOnImage;
  int ret=SINGLE_FIST_FOUND;

  //-- Detect faces
  fist_cascade.detectMultiScale( relevantImgIn, roisOnImage, 1.1, 2, 0|CV_HAAR_SCALE_IMAGE, Size(30, 30) );

  if (roisOnImage.size()<=0)
    {
      // NO FIST FOUND: we aren't tracking anymore
      isTracking=false;
      return NO_FIST_FOUND;
    }
  else
    {
      // otherwise we've found the item and we are tracking from now on
      isTracking=true;
    }

  Rect maxROIFound=roisOnImage[0];
  for( unsigned int i = 1; i < roisOnImage.size(); i++ )
    {
      // woops, many item have been found. We take the biggest one
      ret=MANY_FISTS_FOUND;
      // comparaison with heights of faces
      if (roisOnImage[i].height>maxROIFound.height)
	{
	  maxROIFound=roisOnImage[i];
	}
    }


  // if (inputImageAltered)
  //   {
  //     // we were already tracking:
  //     // we have to frame the ROI we've found within 
  //     // the previous ROI of the image
  //     Point decayROI(ROI.width,ROI.height);
  //     maxROIFound = maxROIFound + decayROI;
  //   }

  ROI=maxROIFound;
  computeROIPosition();

  return ret;
}

void ItemDetecter::computeROIPosition()
{
  // return center of the rect
  center.x=ROI.x+ROI.width/2;
  center.y=ROI.y+ROI.height/2;
}

Mat ItemDetecter::extractROI(Mat img) const
{
  if(isTracking)
    return img(ROI);
  else 
    return img;
}

void getError (int ret)
{
  switch (ret)
  {
  case NO_FIST_FOUND:
    {
      std::cout<<"no fist found"<<std::endl;
    }
    break;
  case SINGLE_FIST_FOUND:
    {
      std::cout<<"one fist found"<<std::endl;
    }
    break;
  case MANY_FISTS_FOUND:
    {
      std::cout<<"many fists found"<<std::endl;
    }
    break;
  }
}

int main (int argc,char** argv)
{
  Point center;
  int ret;

  if (argc>1)
    {
      for (int i=1;i<argc;i++)
	{
	  // créer un flux de sortie
	  std::ostringstream oss;
	  oss<<"test"<<i<<".jpg";
	  std::string name=oss.str();
	  ItemDetecter detecter;
	  Mat frame=imread(argv[i]);
	  ret=detecter.detectAndReframe(frame);
	  getError(ret);
	  if (ret>0)
	    {
	      Mat reframed=detecter.extractROI(frame);
	      imwrite(name,reframed);
	    }
	}
      return 0;
    }
  VideoCapture cap(0); // read video from file
  namedWindow("fist",1);

  if(!cap.isOpened())
    {
      // check if we succeeded
      printf("can't open default webcam\n");
      return -1;
    }

  ItemDetecter detecter;
  for(;;)
    {
      Mat frame;
      cap >> frame; // get a new frame from camera
      cvtColor(frame, frame, CV_BGR2GRAY);
      equalizeHist( frame, frame );

      ret=detecter.detectAndReframe(frame);
      // getError(ret);
      if (ret>0)
      	detecter.drawROI(frame);
      center=detecter.getCenter();
      std::cout<<center<<std::endl;
      imshow("fist", frame);
      imwrite("screenshot.png",detecter.extractROI(frame));
      int key=waitKey(30);
      switch (key)
	{
	case 1048689:
	  {
	    return 0;
	  }
	  break;
	case 1048688:
	  {
	    imwrite("screenshotp.jpg",frame);
	  }
	  break;
	default:
	  {
	    // printf("key : %d\n",key);
	  }
	  break;
	}
    }
  return 0;
}
